#include "can_adapter.h"
#include "can_transreceiver.h"
#include "sensor_utils.hpp"

using namespace sensor::radar;
using namespace sensor::utils;

CanAdapter::CanAdapter(std::unordered_map<std::string, std::shared_ptr<CanTransreceiver>> devs, const std::string& adapter_name)
    : rclcpp::Node(adapter_name)
    , can_devs_(devs)
    , logger_(rclcpp::get_logger(adapter_name))
{
    using namespace std::placeholders;

    for (auto& can : can_devs_) {
        can.second->setReceiveCallback(std::bind(&CanAdapter::onReceiveCanMessage, this, can.first, _1, _2));
    }

    for (size_t i = 0; i < 5; i++) {
        const std::string radar = std::string("radar").append(std::to_string(i + 1));
        radar_obj_pubs_[radar] = this->create_publisher<interface::msg::Objects>(std::string("/sensor/radar/object/") + radar, 10);
        radar_pc_pubs_[radar] = this->create_publisher<interface::msg::PointClouds>(std::string("/sensor/radar/pointCloud/") + radar, 10);
    }
    body_information_pubs_ = this->create_publisher<interface::msg::CarInformation>(std::string("/sensor/car/information"), 10);
    body_information_subs_ = this->create_subscription<interface::msg::CarInformation>("/sensor/car/information", 10,
        std::bind(&CanAdapter::onReceiveInformationMessage, this, _1));
    gnss_subs_["gnss1"] = this->create_subscription<interface::msg::Objects>("/sensor/gnss/location/gnss1", 10,
        std::bind(&CanAdapter::onReceiveGnssMessage, this, _1));
}

void CanAdapter::canSend(std::string dev, int frameid, muduo::net::Buffer& buf)
{
    can_devs_[dev]->send(frameid, buf);
}